#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>

#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/opencv.hpp"


struct AdjustedContour {
    double area;
    cv::Mat approx;
};

class ContourFinder {

private:
    // debugging flags
    bool debugContour; // Print the accepted contours
    bool debugHull; // save copies of images with important contours drawn.
    // Sizes of important stuff
    double minArea;
    double maxArea; // area of concern is largest of the packager sizes ( in each direction)
    double ppi;
    // Mats for intermediate steps
    cv::Mat image;
    cv::Mat centers;
    cv::Mat labels;
    // The contours we find
    std::vector<std::vector<cv::Point> >  contours;
    std::vector<cv::Vec4i> hierarchy;
    std::vector<std::vector<cv::Point> > acceptedContours;
    /**
        do kmeans filtering
    **/
    void prepareImage(cv::Mat img) {
        cv::Mat reshapedImg = img.reshape(1, img.rows * img.cols);
        cv::Mat img32;
        reshapedImg.convertTo(img32, CV_32FC3, 1.0/255.0);
        
        cv::kmeans(img32, 3, labels,
            cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 5, 0.001),
            3, cv::KMEANS_PP_CENTERS, centers);
        std::cout << "Centers: (r,c)" << centers.rows <<" ," << centers.cols << std::endl;
        std::cout << centers << std::endl;
    }
    /**
        convert the labels to something viewable
    **/
    void labels2Image(cv::Mat &img){ 
        image = cv::Mat(img.rows, img.cols, CV_8UC3);
        cv::MatIterator_<cv::Vec3b> rgbPtr = image.begin<cv::Vec3b>();
        cv::MatIterator_<cv::Vec3b> rgbLast = image.end<cv::Vec3b>();
        cv::MatConstIterator_<int> labelPtr = labels.begin<int>();
         
        cv::Mat centersU8;
        centers.convertTo(centersU8, CV_8UC3, 255.0);
         
        while ( rgbPtr != rgbLast ) {
                const cv::Vec3b& rgb = centersU8.ptr<cv::Vec3b>(*labelPtr)[0];
                *rgbPtr = rgb;
                ++rgbPtr;
                ++labelPtr;
        }

    }
    /**
        Filter out uninteresting contours
    **/
    void filterContours() {
      std::vector<cv::Mat> small_1, parent, notConvex, square, large;
        for ( int i = 0; i < contours.size() ; i++) {
            AdjustedContour cont = contourArea(contours[i]);
            bool convex = cv::isContourConvex(cont.approx);
            if ( cont.area/ppi < minArea*.9) { //reject if under 90% of the expected size
                small_1.push_back(cont.approx);
                continue;
            }
            if (hierarchy[i][2] == -1 ) { // must be a parent
                parent.push_back(cont.approx);
                continue;
            }
            if ( debugHull) {
                cv::drawContours( image, contours, i, cv::Scalar( 255,0,0), 3 );
                cv::imwrite("Hull.png", image);
            }
            if ( convex == false ) {//That are closed
                notConvex.push_back(cont.approx);
                continue;
            }
            if ( cont.approx.rows != 4  ) {//only square objects
                square.push_back(cont.approx);
                continue;
            }


            if ( cont.area/ppi > maxArea*1.1) { //reject if larger than 110% of the expected size
                large.push_back(cont.approx);
                continue;
            }
            std::vector<cv::Point> hull;
            cv::convexHull(contours[i], hull, false, true);
            if ( debugHull) {
                cv::drawContours( image, contours, i, cv::Scalar( 255,0,0), 3 );
                cv::imwrite("Hull.png", image);
            }
            cont = contourArea(hull);
            if ( debugContour) {
                //std::clog << log4cpp::Priority::INFO << " Accepted " << cont.area/ppi << " Hull: " << hull << " contour " << cont.approx;
            }
            acceptedContours.push_back(cont.approx);
        }
    }
    /**
        find and filter contours
    **/
    void doContours() {
        cv::findContours( image, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
        filterContours();
        /*for ( int i = 0; i < acceptedContours.size() ; i++) {
            log4cppcout << "contour("<< i << ") " << acceptedContours[i] << log4cppendl;
        }*/

    }
    /**
        convert an image to a mask
    **/
    void createMask(cv::Mat &img) {
        double minVal;
        double medVal;
        double maxVal;
        cv::Point minLoc;
        cv::Point medLoc;
        cv::Point maxLoc;

/*
        labels2Image(img);
*/
        cv::minMaxLoc( centers, &minVal, &maxVal, &minLoc, &maxLoc );
        centers.at<float>(maxLoc) = centers.at<float>(minLoc);
        //log4cppcout << centers << log4cppendl;
        cv::minMaxLoc( centers, &minVal, &medVal, &minLoc, &medLoc );
        //log4cppcout << "min val : " << minVal << log4cppendl;
        //log4cppcout << "med val: " << medVal << log4cppendl;
        //log4cppcout << "max val: " << maxVal << log4cppendl;

        cv::Mat labelsImg = labels.reshape(1, img.rows); // single channel image of labels
        labelsImg.convertTo(labelsImg, img.depth());
        //showscaled("labels", labelsImg, true);
        cv::namedWindow("labels", cv::WINDOW_NORMAL);
        cv::imshow("labels", labelsImg);

        cv::normalize(labelsImg, image, 0, 255, cv::NORM_MINMAX, CV_8UC1);
//        cv::Canny( image, image, minVal, medVal, 5);
        cv::threshold( image, image, 128, 255, 4);
        //showscaled("Mask", image, true);
        cv::namedWindow("Mask", cv::WINDOW_NORMAL);
        cv::imshow("Mask", image);
        //cv::namedWindow("Mask", CV_WINDOW_NORMAL);
        cv::imshow("Mask", image);
        cv::imwrite("Mask.png", image);
        //opencvSave( image, "Mask");

        cv::waitKey();

    }

public:
    // list f contours that we categorized ( public for debugging)
    std::vector<std::vector<cv::Point> > small_1;
    std::vector<std::vector<cv::Point> > square;
    std::vector<std::vector<cv::Point> > parent;
    std::vector<std::vector<cv::Point> > large;
    std::vector<std::vector<cv::Point> > notConvex;

    /**
        Setup for the contour finder
    **/
    ContourFinder( double ia, double xa, double pi): minArea(ia), maxArea(xa), ppi(pi) {}

    /**
        return the list of accepted contours
    **/
    std::vector<std::vector<cv::Point> >  getContours() {
        return acceptedContours;
    }

    /**
        return the list of contours found ( mostly for debugging)
    **/
    std::vector<std::vector<cv::Point> >  getCandidateContours() {
        return contours;
    }

    /**
        approximate a contour based on the given contour
    **/
    static AdjustedContour contourArea(std::vector<cv::Point> c) {
        double epsilon = 0.1*cv::arcLength(c,true);
        AdjustedContour cont;
        cont.area = cv::contourArea(c);
        cv::approxPolyDP(c, cont.approx,epsilon,true);
        return(cont);
    }
    /**
        find all the contours in img. Print them out if pc is true
    **/
    void find( cv::Mat img, bool pc = false) {
        bool dc = debugContour;
        debugContour = pc;
        prepareImage(img);
        createMask(img);
        doContours();
        debugContour = dc;
    }
    bool debugContours( bool state) { debugContour = state; return(debugContour);}
    bool debugHulls( bool state) { debugHull = state; return(debugHull);}

};
